﻿using CG5_KT.Primitives;
using System.Collections.Generic;
using System;

namespace CG5_KT.Scene
{
    public class Route
    {
        private List<Vector3> controlPoints;
        private float tau;

        private int segSplitsCount;
        private int currSegNum;
        private int currSegSplitNum;

        public bool IsDrawPathFlag { get; set; }
        public bool IsDrawCtrlPtsFlag { get; set; }

        public Route()
        {
            this.controlPoints = new List<Vector3>();
            // this.tau = 0.75f;
            this.tau = 0.7f;

            this.segSplitsCount = 25;
            this.currSegSplitNum = 0;
            this.currSegNum = 0;

            this.IsDrawPathFlag = true;
            this.IsDrawCtrlPtsFlag = true;

            this.controlPoints.Add(new Vector3( 0,      0,   2));
            this.controlPoints.Add(new Vector3(-1, 0,  1));
            this.controlPoints.Add(new Vector3(-1, 0.5f,  0));
            this.controlPoints.Add(new Vector3(-1, 0, -1));
            this.controlPoints.Add(new Vector3(-1, 0,  -2));
            this.controlPoints.Add(new Vector3(-0.5f, 0, -5));
            this.controlPoints.Add(new Vector3( 0, 0, -3));
            this.controlPoints.Add(new Vector3( 1, 0.5f, -2));
            this.controlPoints.Add(new Vector3( 2, 0, -1));
            this.controlPoints.Add(new Vector3( 2, 0, 0));
            this.controlPoints.Add(new Vector3( 2, 0, 2));
            this.controlPoints.Add(new Vector3( 1, 0, 3));
            this.controlPoints.Add(new Vector3( 1, 0, 2));
        }

        public bool AssignSegFuncCoeffs(int segNum, ref Vector3 c0, ref Vector3 c1, ref Vector3 c2, ref Vector3 c3)
        {
            var p1 = new Vector3();
            var p2 = new Vector3();
            var p3 = new Vector3();
            var p4 = new Vector3();

            if(this.AssignSegPts(segNum, ref p1, ref p2, ref p3, ref p4))
            {
                c0 = new Vector3(p2                                               );
                c1 = new Vector3(p1*(-tau)                 + p3*tau               );
                c2 = new Vector3(p1*2*tau  +  p2*(tau-3)   + p3*(3-2*tau) - p4*tau);
                c3 = new Vector3(p1*(-tau) +  p2*(2-tau)   + p3*(tau-2)   + p4*tau);

                return true;
            }

            return false;
        }

        public bool AssignSegPts(int segNum, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 p4)
        {
            if (this.controlPoints.Count < 2)
            {
                // At least 2 control points required to form a cyclic route.
                return false;
            }

            int locSegNum = segNum % (controlPoints.Count);
            int cp1 = locSegNum < 1 ? controlPoints.Count - 1 : locSegNum - 1;
            int cp2 = locSegNum;
            int cp3 = (locSegNum + 1) % controlPoints.Count;
            int cp4 = (locSegNum + 2) % controlPoints.Count;

            p1 = new Vector3(this.controlPoints[cp1]);
            p2 = new Vector3(this.controlPoints[cp2]);
            p3 = new Vector3(this.controlPoints[cp3]);
            p4 = new Vector3(this.controlPoints[cp4]);

            return true;
        }

        public void NextSegSplitStep()
        {
            this.currSegSplitNum++;

            if (this.currSegSplitNum < this.segSplitsCount)
            {
                return;
            }

            // Reached segSplitsCount, time to switch to next segment and restart from its beginning.
            this.currSegSplitNum = 0;

            this.currSegNum++;

            if (this.currSegNum == this.controlPoints.Count)
            {
                this.currSegNum = 0;
            }
        }

        public void PrevSegSplitStep()
        {
            this.currSegSplitNum--;

            if (this.currSegSplitNum >= 0)
            {
                return;
            }

            this.currSegSplitNum = this.segSplitsCount - 1;
            this.currSegNum--;

            if (this.currSegNum < 0)
            {
                currSegNum = this.controlPoints.Count - 1;
            }
        }

        private void DrawControlPts(Engine engine, Matrix transformMatrix)
        {
            for (int i = 0; i < this.controlPoints.Count; i++)
            {
                var vertex = new Vertex();
                vertex.Coordinates = new Vector3(this.controlPoints[i].X, this.controlPoints[i].Y, this.controlPoints[i].Z);

                var point = engine.Project(vertex, transformMatrix, Matrix.Identity);
                engine.DrawPoint(point.Coordinates, new Color4(1, 0, 0, 1));
            }
        }

        private void DrawRoute(Engine engine, Matrix transformMatrix)
        {
            if (this.controlPoints.Count < 2)
            {
                // Cycles allowed => at least 2 control points. Otherwise, at least 4 are required.
                return;
            }

            for (int i = 0; i < this.controlPoints.Count; i++)
            {
                var c0 = new Vector3();
                var c1 = new Vector3();
                var c2 = new Vector3();
                var c3 = new Vector3();

                if (this.AssignSegFuncCoeffs(i, ref c0, ref c1, ref c2, ref c3) == false)
                {
                    return;
                }

                for (float t = 0; t <= 1; t += 0.01f)
                {
                    var res = c0 + c1*t + c2*t*t + c3*t*t*t;

                    var vertex = new Vertex();
                    vertex.Coordinates = new Vector3(res.X, res.Y, res.Z);

                    var point = engine.Project(vertex, transformMatrix, Matrix.Identity);
                    engine.DrawPoint(point.Coordinates, new Color4(1, 0, 0, 1));
                }
            }
        }

        public void Draw(Engine engine, Matrix transformMatrix)
        {
            if (this.IsDrawPathFlag == true)
            {
                this.DrawControlPts(engine, transformMatrix);
            }

            if (this.IsDrawPathFlag == true)
            {
                this.DrawRoute(engine, transformMatrix);
            }
        }

        public Vector3 CalcAircraftPos()
        {
            float t = 0;
            Vector3 p0 = new Vector3();
            Vector3 c0 = new Vector3();
            Vector3 c1 = new Vector3();
            Vector3 c2 = new Vector3();
            Vector3 c3 = new Vector3();

            if (this.AssignSegFuncCoeffs(this.currSegNum, ref c0, ref c1, ref c2, ref c3) == false)
            {
                return new Vector3();
            }

            t = (1.0f / this.segSplitsCount) * this.currSegSplitNum;
            p0 = c0 + c1*t + c2*t*t + c3*t*t*t;

            return p0;
        }

        public Vector3 CalcAircraftYawAndPitch()
        {
            // Calculate position from the preceding frame.
            this.PrevSegSplitStep();
            Vector3 p0 = this.CalcAircraftPos();

            // Calculate position from the current frame.
            this.NextSegSplitStep();
            Vector3 p1 = this.CalcAircraftPos();

            Vector3 pDirection = p1 - p0;

            // Axis X rotation (Pitch).
            float tanYZ = pDirection.Y / pDirection.Z;
            float radsYZ = -(float)Math.Atan(tanYZ);

            if (p1.Z >= p0.Z)
            {
                // In case aircraft moves towards Z (Z increases for the next step) then calculated angle sign
                // has to be changed. Otherwise, aircaft nose goes up while it should go down.
                radsYZ *= -1;
            }

            // Axis Y rotation (Yaw).
            float tanZX = pDirection.Z / pDirection.X;
            float radsZX = (float)Math.Atan(tanZX);

            if (p1.X >= p0.X)
            {
                // If going right then rotate model by 90 clockwise.
                radsZX += -(-90 * (float)Math.PI / 180);
            }
            else
            {
                // Otherwise rotate counter clock-wise.
                radsZX += (-90 * (float)Math.PI / 180);
            }

            return new Vector3(radsYZ, -radsZX, 0);
        }
    }
}
